/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 11:37:47
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-06 22:27:41
 * @Description: header
 * @FilePath: \fzhtq\include\caldrive.h
 */

#ifndef CALDRIVE_H_
#define CALDRIVE_H_

/**
 * @description: 计算动量轮参数
 * @algorithm: 
 * @param {double} WhlSpdNow
 * @param {double} WhlSpdDsrd
 * @param {double} WhlSpdDsrdLst
 * @param {double} whlAxiAngMom
 * @param {double} WhlAxisTq
 * @return {*}
 */
void CalWhl(double WhlSpdNow[4], double WhlSpdDsrd[4], double WhlSpdDsrdLst[4], double whlAxiAngMom[3], double WhlAxisTq[3]);

/**
 * [计算控制力矩（各个动量轮）]
 * @param [WhlTq] [计算后的力矩]
 * @param [WhlAccel] [动量轮加速度]
 * @param [ratio] [加速度与控制力矩的比例]
 * (WhlTq为输出，其它为输入)
 */
void CalWhlTq(double whlTq[4], double whlacc[4], double ratio);

/**
 * [计算动量轮转动惯量]
 * @param [WhlAngMom] [转动惯量]
 * @param [WhlSpdNow] [动量轮转速]
 * @param ratio  [转动惯量与转动速度的比例关系]
 */
void CalWhlMom(double whlangmom[4], double whlspdnow[4], double ratio);

/**
 * [计算推力器用于姿控的力矩]
 * @param [M_PS] [计算后的力矩]
 * @param [thrusterAtt] 推力器力矩矩阵
 * @param [PS_PTime] [推力器作用时间]
 * (M_PS为输出，其它为输入)
 */
void CalAttCtrlPS(double Mps[3], double thrusterAtt[3][10], double pstime[10]);

#endif
